/*******************************************************************************
* FILE NAME: user_routines_fast.c <VEX VERSION>
*
* DESCRIPTION:
*  This file is where the user can add their custom code within the framework
*  of the routines below. 
*
* USAGE:
*  You can either modify this file to fit your needs, or remove it from your 
*  project and replace it with a modified copy. 
*
* OPTIONS:  Interrupts are disabled and not used by default.
*
*******************************************************************************/

#include "ifi_aliases.h"
#include "ifi_default.h"
#include "ifi_utilities.h"
#include "user_routines.h"
#include "printf_lib.h"
#include "delays.h"


/*** DEFINE USER VARIABLES AND INITIALIZE THEM HERE ***/
int autonomous_Stage;
signed int right_Wheel_Counter;
signed int left_Wheel_Counter;
unsigned long floor_Value_Sum;
int floor_Counter;
int floor_Value_Average;
unsigned long ultra_Sonic_Sum;
int ultra_Sonic_Counter;
int ultra_Sonic_Average;
#define BUTTON_FWD_THRESH       154
#define NEUTRAL_VALUE           127

//int difference_Limit;
char tape_Flag;
char triggerCnt;
unsigned char myval;              /* Used to create a 13us pulse */
unsigned char HiResCounter;		//count while sonar pulse is out.
unsigned char LowResCounter;
unsigned char fltHiResCounter;
unsigned char fltLowResCounter;
unsigned char fltCounter;
unsigned char SonarDataValid;
unsigned char Delta;
int lift_Position;
int lift_Counter;
int lift_Height;
int lift_Time;
extern long leftError, rightError;


/*******************************************************************************
* FUNCTION NAME: InterruptVectorLow
* PURPOSE:       Low priority interrupt vector
* CALLED FROM:   nowhere by default
* ARGUMENTS:     none
* RETURNS:       void
* DO NOT MODIFY OR DELETE THIS FUNCTION 
*******************************************************************************/
#pragma code InterruptVectorLow = LOW_INT_VECTOR
void InterruptVectorLow (void)
{
  _asm
    goto InterruptHandlerLow  /*jump to interrupt routine*/
  _endasm
}


/*******************************************************************************
* FUNCTION NAME: InterruptHandlerLow
* PURPOSE:       Low priority interrupt handler
* If you want to use these external low priority interrupts or any of the
* peripheral interrupts then you must enable them in your initialization
* routine.  Innovation First, Inc. will not provide support for using these
* interrupts, so be careful.  There is great potential for glitchy code if good
* interrupt programming practices are not followed.  Especially read p. 28 of
* the "MPLAB(R) C18 C Compiler User's Guide" for information on context saving.
* CALLED FROM:   this file, InterruptVectorLow routine
* ARGUMENTS:     none
* RETURNS:       void
*******************************************************************************/
#pragma code
#pragma interruptlow InterruptHandlerLow save=PROD /* You may want to save additional symbols. */

void InterruptHandlerLow ()     
{                               
  unsigned char int_byte;  
     
  //=============SONAR RANGE OUTPUT=================
  if (INTCON3bits.INT2IF && INTCON3bits.INT2IE)       /* The INT2 pin is RB2/DIG I/O 1. */
  { 
	//right_Wheel_Counter ++; /*TODO: We need to move this out of INT2*/
	INTCON3bits.INT2IF = 0;   /*Reset the flag (we got it)*/
    Handle_EchoPulse_From_Sonar_Device(); 
  }

  // ===============LEFT WHEEL COUNTER================
  else if (INTCON3bits.INT3IF && INTCON3bits.INT3IE)  /* The INT3 pin is RB3/DIG I/O 2. */
  {
    if(left_Wheel_Backwards_Check)
	{
		left_Wheel_Counter ++;
	}
	
	else
	{
		left_Wheel_Counter ++;
	}
	
	INTCON3bits.INT3IF = 0;
  }
  //================ON CHIP TIMER 0 =================
  else if ( (INTCONbits.TMR0IE) && (INTCONbits.TMR0IF) ) /*Timer flag enabled & flag is set*/
  {
	//printf("Just caught a an TMR0 ");
    INTCONbits.TMR0IF = 0;
    HiResCounter++;
  }
  //================RIGHT WHEEL COUNTER==============
  else if (INTCONbits.RBIF && INTCONbits.RBIE)  /* DIG I/O 3-6 (RB4, RB5, RB6, or RB7) changed. */
  {
   	if (rc_dig_int3 == 0)
	{
		if(right_Wheel_Backwards_Check)
		{
			right_Wheel_Counter ++;
		}
	
		else
		{
			right_Wheel_Counter --;
		}
	}
    int_byte = PORTB;          /* You must read or write to PORTB */
    INTCONbits.RBIF = 0;     /*     and clear the interrupt flag         */
  }                                        /*     to clear the interrupt condition.  */
}



/*******************************************************************************
* FUNCTION NAME: User_Autonomous_Code
* PURPOSE:       Execute user's code during autonomous robot operation.
* You should modify this routine by adding code which you wish to run in
* autonomous mode.  It will be executed every program loop, and not
* wait for or use any data from the Operator Interface.
* CALLED FROM:   main.c file, main() routine when in Autonomous mode
* ARGUMENTS:     none
* RETURNS:       void
*******************************************************************************/
void User_Autonomous_Code(void)
{
  /* Initialize all PWMs and Relays when entering Autonomous mode, or else it
     will be stuck with the last values mapped from the joysticks.  Remember, 
     even when Disabled it is reading inputs from the Operator Interface. 
  */
  LEFT_WHEEL_MOTOR = RIGHT_WHEEL_MOTOR = pwm03 = pwm04 = 127;
  pwm05 = pwm06 = INTAKE_MOTOR = LEFT_LIFT_MOTOR = 127;
  pwm09 = pwm10 = pwm11 = pwm12 = 127;
  

  while (autonomous_mode)   /* DO NOT CHANGE! */
  {

	//printf("autonomous_Stage: %2u \n", autonomous_Stage);

	if (statusflag.NEW_SPI_DATA)      //* 18.5ms loop area 0
    {
      Getdata(&rxdata);   //* DO NOT DELETE, or you will be stuck here forever! 

		//848 is the height of the 21 inch tube, 760 is the height of the 15 inch tube
		//TODO:  The right side goes further than 180 before stopping. (about 250)
	
		if (autonomous_Stage == 1)
		{
			lift_N_Push(760, 90);  //lift to height of (760) for  push time (90) into 15in tube
	  	}

		if (autonomous_Stage == 2)
		{
			turn(LEFT, 170, 800);   //turn (left) for (162) gear teeth and bring lift to (800)
		} 

		if (autonomous_Stage == 3)
		{
			drive_Straight(80, 345, 0);   //drive straight at speed (80) for (345) gear teeth until proximity (0)
	  	}

		if (autonomous_Stage == 4)
		{
			lift_N_Push(559, 60);   //lift to height of (559) for push time (60) into 9in tube
		}
		
		if (autonomous_Stage == 5)
		{
			turn(LEFT, 130, 600);	//turn (left) for (130) gear teeth and bring lift to (600)
		}
		
		if (autonomous_Stage == 6)
		{
			block_Intake(100, 700); //intake blocks while moving forward at speed (80) for (700) gear teeth
		}
	
		if (autonomous_Stage == 7)
		{
			turn(RIGHT, 230, 700); //turn (right) for (230) gear teeth and bring lift to (700)
		}

		if (autonomous_Stage == 8)
		{
			drive_Straight(80, 250, 0); //drive straight at speed (80) for (250) gear teeth until proximity (0)
		}

		if (autonomous_Stage == 9)
		{
			lift_N_Push(700,100); //lift to height of (700) for push time (100) outside of wall
		}

	printf("Astage: %u, Left C: %u, Left M: %u, Right C: %u, Right M: %u liftpos: %u  \n",
			autonomous_Stage,left_Wheel_Counter,LEFT_WHEEL_MOTOR,right_Wheel_Counter,RIGHT_WHEEL_MOTOR,lift_Position);

	Putdata(&txdata);   /* DO NOT DELETE, or you will get no PWM outputs! */
    }
  }
}




/*******************************************************************************
* FUNCTION NAME: Process_Data_From_Local_IO
* PURPOSE:       Execute user's realtime code.
* You should modify this routine by adding code which you wish to run fast.
* It will be executed every program loop, and not wait for fresh data 
* from the Operator Interface.
* CALLED FROM:   main.c
* ARGUMENTS:     none
* RETURNS:       void
*******************************************************************************/
void Process_Data_From_Local_IO(void)
{
  /* Add code here that you want to be executed every program loop. */

}



/*******************************************************************************
* FUNCTION NAME: find_Floor_Average
* PURPOSE:       Find average of a constant flow of numbers and change floor
				 values
* CALLED FROM:   This file
* ARGUMENTS:     number
* RETURNS:       void
*******************************************************************************/
void find_Floor_Average(unsigned int Recieved_Number)
{
	floor_Value_Sum = floor_Value_Sum + Recieved_Number;
	floor_Counter ++;
	floor_Value_Average = floor_Value_Sum / floor_Counter;
}

/*******************************************************************************
* FUNCTION NAME: find_UltraSonic_Average
* PURPOSE:       Find average of a constant flow of numbers and change the value
				 of the ultra sonic sensor
* CALLED FROM:   This file
* ARGUMENTS:     number
* RETURNS:       void
*******************************************************************************/
void find_UltraSonic_Average(unsigned int Recieved_Number)
{
	ultra_Sonic_Sum = ultra_Sonic_Sum + Recieved_Number;
	ultra_Sonic_Counter ++;
	ultra_Sonic_Average = ultra_Sonic_Sum / ultra_Sonic_Counter;
}

/*******************************************************************************
* FUNCTION NAME: Handle_EchoPulse_From_Sonar_Device
* PURPOSE:       Starts a timer to time the sonar pulse.  Decides whether the time is valid or not
* CALLED FROM:   This file
* ARGUMENTS:     None
* RETURNS:       void
*******************************************************************************/
void Handle_EchoPulse_From_Sonar_Device(void)
{
  /* Setup to start counting timer pulses*/
  if (INTCON2bits.INTEDG2)      /* If we catch a rising edge signal, 
								   (pulse just went out) (This will only happen once per pulse)*/
  {                             /* Yes */
    //printf("Just caught a rising edge on INT2 in handle_EchoPulse");
    T0CONbits.T0PS0 = 1;        /* Change scale to DIV 8 (204us delay)*/
    TMR0H = TMR0L = 0;          /* reset timer registers (reset timer) */
    INTCONbits.TMR0IF = 0;      /* make sure tmr flag is clear (Don't keep firing on INT2 interrupts (interrupt pin1 */
    INTCONbits.TMR0IE = 1;      /* Enable TMR0 Int (Turn on the timer interrupt) */
    INTCON2bits.INTEDG2 = 0;    /* trigger of falling edge (Look for INT2 (interrupt pin1 falling edge)
								   (signal pulse just came back) */
    HiResCounter = LowResCounter = 0; /* Reset Counters  */
  }

  /*(We just caught a falling edge signal)(sonar pulse just came back)*/
  else
  {
	//printf("Just caught a falling edge on INT2 in handle_EchoPulse");
    INTCON3bits.INT2IE = 0;     /* disable rc_dig_int1(INT2)(Interrupt pin1) for now */
    LowResCounter = TMR0L;      /*This is the low byte from the timer*/
    INTCONbits.TMR0IE = 0;      /* disable TMR0 (Timer) Int */

    /* Now apply a filter to data */
    if (HiResCounter < 100) /*Make sure we haven't been counting too long(too far away)*/
    {
      if (fltHiResCounter == HiResCounter) /*If the current reading matches up with the average*/
      {
        SonarDataValid = 1;				//The current reading is valid
        fltCounter = 0; 				/*Reset*/
        Delta = fltLowResCounter - LowResCounter;  //difference from the baseline to the low byte of the current reading
        if (Delta & 0x80)   /* If negative then negate it */
          Delta = !Delta;
        if (Delta > 20)
          fltCounter++;		//count this toward the filter count.
      }
      else /*If the current reading doesn't up with the average*/
      {
        fltCounter++;  //We have filtered another reading
      }

      if (fltCounter > 3) 					//If we have filtered more than 3 in a row
      {										//Set a new baseline
         SonarDataValid = 1;				//The current reading is valid
         fltHiResCounter = HiResCounter;	//This is the new value
         fltLowResCounter = LowResCounter;
      }
    }
	else
	{
		SonarDataValid = 0;
	}
	printf("fltHiRes: %u, fltLowRes: %u ;; HiRes: %u,  LowRes: %u, valid: %u \n",fltHiResCounter, fltLowResCounter,HiResCounter, LowResCounter,SonarDataValid);
  }
}

/*******************************************************************************
* FUNCTION NAME: send_Sonar_Start_Signal
* PURPOSE:       Tells the sonar to send out a pulse (rc_dig_out04)
* CALLED FROM:   This file
* ARGUMENTS:     None
* RETURNS:       void
*******************************************************************************/
void send_Sonar_Start_Signal(void){
	//printf("We Just came into a timer trigger IF statement. \n");
    INTCON2bits.INTEDG2 = 1;  /* rc_dig_int1 - Interrupt on rising edge */
    INTCONbits.TMR0IE = 0;    /* make sure TMR0 Int is off */
    T0CONbits.T0PS0 = 0;	//set prescale value 1:2
    TMR0H = 0;				//reset high byte
    INTCONbits.TMR0IF = 0;
	myval = 0xC0;             /* Yields a 13us delay */    
	TMR0L = myval;			//set low byte to 11000000 , 0octal4000
    rc_dig_out04 = 1;         /* prepare a 13us pulse - rising edge of pulse */
	//printf("We have sent a start pulse-. \n");
    while (!INTCONbits.TMR0IF){  	//Wait for our 13us delay to expire and trigger the interrupt flag
		//printf("We are in a while loop waiting for timer flag to go on. \n");
    }
    rc_dig_out04 = 0;          /* falling edge of pulse */
    triggerCnt = 0;           /* reset trigger counter */
    INTCON3bits.INT2IE = 1;   /* enable rc_dig_int1*/
}

/*******************************************************************************
* FUNCTION NAME: go_To_Line
* PURPOSE:       Go to goal line
* CALLED FROM:   This file
* ARGUMENTS:     None
* RETURNS:       void
*******************************************************************************/
void go_To_Line(void)
{
	RIGHT_WHEEL_MOTOR = 127 + 127;
	LEFT_WHEEL_MOTOR = 127 - 127;
	find_Floor_Average(Get_Analog_Value(rc_ana_in01));
		
	if ((left_Wheel_Counter + right_Wheel_Counter) / 2 > 100) //(floor_Counter > 100)  //200 was 540
	{
		RIGHT_WHEEL_MOTOR = 127 + 50;
		LEFT_WHEEL_MOTOR = 127 - 50;

		if ((signed int)Get_Analog_Value(rc_ana_in01) - (signed int)floor_Value_Average > 300 || (signed int)floor_Value_Average - (signed int)Get_Analog_Value(rc_ana_in01) > 300 || tape_Flag == 1)
		{
			endStage();
			tape_Flag = 1;
		}
	}
}

/*******************************************************************************
* FUNCTION NAME: turn_To_Block
* PURPOSE:       Turn to block either left or right
* CALLED FROM:   This file
* ARGUMENTS:     left or right
* RETURNS:       void
* NOTE:			use fltHiResCounter as a distance reading and only if SonarDataValid == 1
*******************************************************************************/
void turn_To_Block(char direction)
{
	RIGHT_WHEEL_MOTOR = 127 + 50;
	LEFT_WHEEL_MOTOR = 127 + 50;
	triggerCnt++;
 	
	if (triggerCnt > 2) //* expires every 4 * 18.5ms *
	{         
		send_Sonar_Start_Signal(); //Assumes the sonar start signal goes out on rc_dig_out04
   	}
	if (1) //SonarDataValid) //If Sonar data is valid drive the robot until it comes close to a wall
	{  
  		SonarDataValid = 0;  //invalidate the sonar reading (reset)
   		printf("H %d, L %d, D %d\n",(int)fltHiResCounter,(int)fltLowResCounter,(int)Delta);  
	}
    //	printf("%2x :m: %3u %3u ;ave:  %u  cur: %u  n: %u \n",(int)rxdata.rc_receiver_status_byte.allbits,pwm03,RIGHT_WHEEL_MOTOR, floor_Value_Average,
    //	Get_Analog_Value(rc_ana_in01), floor_Counter);
			
	find_UltraSonic_Average(HiResCounter);
				
	if ((signed int)HiResCounter - (signed int)ultra_Sonic_Average > 20 || (signed int)ultra_Sonic_Average - (signed int)HiResCounter > 20)
	{
		RIGHT_WHEEL_MOTOR = 127 - 50;
		LEFT_WHEEL_MOTOR = 127 + 50;
				
		if (HiResCounter < 15)
		{
			endStage();
		}
	}
}

/*******************************************************************************
* FUNCTION NAME: lift_N_Push
* PURPOSE:       Lift the catcher and empty the blocks
* CALLED FROM:   This file
* ARGUMENTS:     height, push_Time
* RETURNS:       void
*******************************************************************************/
void lift_N_Push(int lift_Height,int push_Time)
{
	pwm06 = 127;
	INTAKE_MOTOR = 127;
	
	lift_Position = get_Lift_Position();

	if (lift_Position < lift_Height && difference(lift_Position, lift_Height) > 50)
	{
		LEFT_LIFT_MOTOR = 127 + 100;
	}

	else if (lift_Position > lift_Height && difference(lift_Position, lift_Height) > 50)
	{
		LEFT_LIFT_MOTOR = 127 - 20;
	}

	if (lift_Counter < push_Time && difference(lift_Height, lift_Position) < 50) // reduce or increase the lift counter to make it shoter or longer
	{					  // Change the number either higher
		LEFT_LIFT_MOTOR = 127 + 40;
		INTAKE_MOTOR = 127 - 60; // or lower to make motors go faster or slower
		lift_Counter ++;
	}
	
	else if (lift_Counter >= push_Time)
	{
		lift_Counter = 0;
		endStage();
	}
		
}

/*******************************************************************************
* FUNCTION NAME: difference
* PURPOSE:       returns the difference of 2 numbers
* CALLED FROM:   This file
* ARGUMENTS:     2 numbers
* RETURNS:       number
*******************************************************************************/
int difference(int A, int B)
{
	if (A > B)
	{
		return A - B;
	}
	
	if (B > A)
	{
		return B - A;
	}

	if (A == B)
	{
		return 0;
	}
}

/*******************************************************************************
* FUNCTION NAME: turn
* PURPOSE:       turn either left or right
* CALLED FROM:   This file
* ARGUMENTS:     direction (0 == left, 1 == right)
* RETURNS:       void
*******************************************************************************/
void turn(char direction, int turn_Amount, int lift_Pos)
{
	//right_Wheel_Counter = 0;
	//left_Wheel_Counter = 0;
	INTAKE_MOTOR = 127;
	lift_Position = get_Lift_Position();
	if (lift_Position < lift_Pos)
	{
		LEFT_LIFT_MOTOR = 127 + 110;
	}
	else 
	{
		LEFT_LIFT_MOTOR = 127 + 30;
	}
	if (direction == LEFT)
	{
		LEFT_WHEEL_MOTOR = 127 - 95;
		RIGHT_WHEEL_MOTOR = 127 - 95;
	}

	else if (direction == RIGHT)
	{
		LEFT_WHEEL_MOTOR = 127 + 95;
		RIGHT_WHEEL_MOTOR = 127 + 95;
	}
	
	if (right_Wheel_Counter > turn_Amount)
	{
		RIGHT_WHEEL_MOTOR = 127;
	}

	if (left_Wheel_Counter > turn_Amount)
	{
		LEFT_WHEEL_MOTOR = 127;
	}

	if (right_Wheel_Counter > turn_Amount && left_Wheel_Counter > turn_Amount)
	{
		endStage();
	}
}

/*******************************************************************************
* FUNCTION NAME: drive_Straight
* PURPOSE:       Lift the catcher and empty the blocks
* CALLED FROM:   This file
* ARGUMENTS:     speed, length
* RETURNS:       void
*******************************************************************************/
void drive_Straight(int speed, int length, int proximity)
{
	//right_Wheel_Counter = 0;
	//left_Wheel_Counter = 0;	
	
	if (proximity)
	{
	triggerCnt++;
 	
		if (triggerCnt > 2) //* expires every 4 * 18.5ms *
		{         
			send_Sonar_Start_Signal(); //Assumes the sonar start signal goes out on rc_dig_out04
   		}	
	}
	
	if (speed > 127)
	{
		speed = 127;
	}

	if (speed < 10)
	{
		speed = 10;
	}	

	LEFT_WHEEL_MOTOR = 127 + speed;
	RIGHT_WHEEL_MOTOR = 127 - speed;	

	if (difference(right_Wheel_Counter, left_Wheel_Counter) > 1)
	{
		if (right_Wheel_Counter > left_Wheel_Counter)
		{	
			RIGHT_WHEEL_MOTOR = RIGHT_WHEEL_MOTOR + speed / 2;
		}	
		
		else
		{
			LEFT_WHEEL_MOTOR = LEFT_WHEEL_MOTOR - speed / 2;
		}	
	}
	if(length != 0 && right_Wheel_Counter > length && left_Wheel_Counter > length)
	{
		endStage();
	}
	
	else if(proximity != 0 && fltHiResCounter < proximity)
	{
		endStage();
	}
}
/*******************************************************************************
* FUNCTION NAME: block_Intake
* PURPOSE:       Suck in blocks
* CALLED FROM:   This file
* ARGUMENTS:     Speed, Distance
* RETURNS:       void
*******************************************************************************/
void block_Intake(char speed,int distance)
{
	lift_Position = get_Lift_Position();
	if (lift_Position > 345)
	{
		LEFT_LIFT_MOTOR = 127 - 70;
	}
	
	else
	{
		drive_Straight(speed, distance, 0);
		INTAKE_MOTOR = 127 + 127;
		LEFT_LIFT_MOTOR = 127 + 10;

		/*if (left_Wheel_Counter < distance  && right_Wheel_Counter < distance)
		{
			LEFT_WHEEL_MOTOR = RIGHT_WHEEL_MOTOR =  speed ;
			INTAKE_MOTOR = 127 + speed;
		}
		else (Ive moved more than distance)
		{
			stop moving;
		}*/
	}
}


/*******************************************************************************
* FUNCTION NAME: endStage
* PURPOSE:       reset variables and move stage counter up one
* CALLED FROM:   This file
* ARGUMENTS:     none
* RETURNS:       void
*******************************************************************************/
void endStage(void)
{
	//Stop the motors
	LEFT_WHEEL_MOTOR = 127;
	RIGHT_WHEEL_MOTOR = 127;
	//Reset the counters
	right_Wheel_Counter = 0;
	left_Wheel_Counter = 0;
	//Reset the error count
	rightError = 0;
	leftError = 0;
	//Move on to the next stage
	autonomous_Stage ++;
}